//
// Created by juangeng on 17/07/19.
//

#ifndef POS_GENERATE_ORITRANS_H
#define POS_GENERATE_ORITRANS_H

#include <Eigen/Dense>
#include <cmath>

class OriTrans {
public:
    static void Euler2DCM(Eigen::Vector3d Euler, Eigen::Matrix3d& DCM) {
        double pitch = Euler(0), roll = Euler(1), yaw = Euler(2);
        double c_11 = cos(roll) * cos(yaw);
        double c_12 = -cos(pitch) * sin(yaw) + sin(pitch) * sin(roll) * cos(yaw);
        double c_13 = sin(pitch) * sin(yaw) + cos(pitch) * sin(roll) * cos(yaw);
        double c_21 = cos(roll) * sin(yaw);
        double c_22 = cos(pitch) * cos(yaw) + sin(pitch) * sin(roll) * sin(yaw);
        double c_23 = -sin(pitch) * cos(yaw) + cos(pitch) * sin(roll) * sin(yaw);
        double c_31 = -sin(roll);
        double c_32 = sin(pitch) * cos(roll);
        double c_33 = cos(pitch) * cos(roll);
        DCM << c_11, c_12, c_13,
                c_21, c_22, c_23,
                c_31, c_32, c_33;
    }

    //输入：　四元素，　输出：姿态矩阵，　单位：rad
    static void Quater2DCM(Eigen::Quaterniond Quater, Eigen::Matrix3d& DCM) {
        double q0 = Quater.w(), q1 = Quater.x(), q2 = Quater.y(), q3 = Quater.z();
        DCM << q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3, 2 * (q1 * q2 - q0 * q3), 2 * (q1 * q3 + q0 * q2),
                2 * (q1 * q2 + q0 * q3), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3, 2 * (q2 * q3 - q0 * q2),
                2 * (q1 * q3 - q0 * q2), 2 * (q2 * q3 + q0 * q1), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
    }

    //输入：欧拉角，　输出：四元素，　单位：rad
    static void Euler2Quater(Eigen::Vector3d Euler, Eigen::Quaterniond& Quater) {
        double e1 = Euler(0) / 2, e2 = Euler(1) / 2, e3 = Euler(2) / 2;
        Quater.w() = cos(e1) * cos(e2) * cos(e3) + sin(e1) * sin(e2) * sin(e3);
        Quater.x() = sin(e1) * cos(e2) * cos(e3) - cos(e1) * sin(e2) * sin(e3);
        Quater.y() = cos(e1) * sin(e2) * cos(e3) + sin(e1) * cos(e2) * sin(e3);
        Quater.z() = cos(e1) * cos(e2) * sin(e3) - sin(e1) * sin(e2) * cos(e3);
    }

    //输入：姿态矩阵，　输出：欧拉角，　单位：rad
    static void DCM2Euler(Eigen::Matrix3d DCM, Eigen::Vector3d& Euler) {
        //e1(-PI, PI), e2(-PI/2, PI/2), e3(-PI, PI)
        double e1 = atan2(DCM(2, 1), DCM(2, 2));
        double e2 = atan2(-DCM(2, 0), (pow((pow(DCM(2, 1), 2) + pow(DCM(2, 2), 2)), 0.5)));
        double e3 = atan2(DCM(1, 0), DCM(0, 0));

        Euler << e1, e2, e3;
    }
};

#endif //TRACKINGTEST_ORITRANS_H
